// main.cpp : Defines the entry point for the console application.
//

#include <cml/cml.h>
#include <vector>
#include <ode/ode.h>
#include <GL/glut.h>
#include <GL/gl.h>
#include <CImg-1.3.1/CImg.h>
#include "Box.h"
#include "Sphere.h"
#include "Hill.h"
#include "Terrain.h"
#include "Diamond.h"
#include "ImgReader.h"
#include "main.h"

#include "ParticleConstraint.h"
#include "Cloth.h"
//How often to update the scene's physical model in miliseconds
const unsigned int physicsUpdateRate = 4;
//How often to update the scene's animation in miliseconds
const unsigned int animationUpdateRate = 16;

const bool usePlanes = false; // Use a plane instead of loaded terrain?

int startTime;
int prevTime;

dWorldID world;
dSpaceID space;
dJointGroupID contactGroup;

std::vector<dGeomID> planes;
std::vector<StaticTrimesh*> staticTrimeshes;
std::vector<Object*> objects;


std::vector<Box*> particles;
//The trackball is based on a tut I saw but can't find since...
//Trackball related globals
float g_trackball_zoom = 0.0f;
float g_trackball_model_rot_x = 0.0f;
float g_trackball_model_rot_y = 0.001f;
float g_trackball_translation_x = 0.0f;
float g_trackball_translation_y = 0.0f;
int g_trackball_last_x = 0, g_trackball_last_y = 0;
unsigned char g_trackball_buttons[3];

int width = 5, height = 5;
int num_particles_width = 20;
int num_particles_height = 20;
int minTerrainHeight=0;
float boxw =0.05,boxh =0.05, boxd =0.001;
//Box *box = new Box(world, space, 0.05, 0.05, 0.0001, 0.0001);

void handleMouseClick(int button, int state, int x, int y)
{
	g_trackball_last_x = x;
	g_trackball_last_y = y;
	switch(button)
	{
		case GLUT_LEFT_BUTTON:
			g_trackball_buttons[0] = ((GLUT_DOWN == state) ? 1 : 0); //Set button state to 1 if button down or 0 otherwise
			break;
		case GLUT_MIDDLE_BUTTON:
			g_trackball_buttons[1] = ((GLUT_DOWN == state) ? 1 : 0);
			break;
		case GLUT_RIGHT_BUTTON:
			g_trackball_buttons[2] = ((GLUT_DOWN == state) ? 1 : 0);
			break;
		default:
			break;		
	}
	glutPostRedisplay();
}

void handleMouseMotion(int x, int y)
{
	int diff_x = x - g_trackball_last_x;
	int diff_y = y - g_trackball_last_y;
	g_trackball_last_x = x;
	g_trackball_last_y = y;

	if(g_trackball_buttons[0] && g_trackball_buttons[2])
	{
		g_trackball_zoom -= (float) 0.05f * diff_x;
		//Clamp zoom
		if(g_trackball_zoom < -5.0f)
			g_trackball_zoom = -5.0f;
	}
	else
	{
		if(g_trackball_buttons[0])
		{
			g_trackball_model_rot_x += (float) 0.5f * diff_y;
			g_trackball_model_rot_y += (float) 0.5f * diff_x;

			//Keep x rotations within [0, 360)
			while(g_trackball_model_rot_x >= 360.0f)
				g_trackball_model_rot_x -= 360.0f;
			while(g_trackball_model_rot_x < 0.0f)
				g_trackball_model_rot_x += 360.0f;

			//Keep y rotations within [0, 360)
			while(g_trackball_model_rot_y >= 360.0f)
				g_trackball_model_rot_y -= 360.0f;
			while(g_trackball_model_rot_y < 0.0f)
				g_trackball_model_rot_y += 360.0f;
		}
		else
		{
			if( g_trackball_buttons[1] )
			{
				g_trackball_translation_x += (float) 0.05f * diff_x;
				g_trackball_translation_y -= (float) 0.05f * diff_y;
			}
		}
	}
	glutPostRedisplay();
}

void handleKeyboardEvent(unsigned char key, int x, int y)
{
	static bool isCulling = false, isNormalizingNormals = false, isTexture = false;
	switch (key)
	{
		case 'c':
			std::cout << std::boolalpha << "Face culling: " << !isCulling << "\n"; //boolalpha means bools will be true/fale not 1/0
			if(isCulling)
			{
				isCulling = !isCulling;
				glDisable(GL_CULL_FACE);
				glutPostRedisplay();
				break;
			}
			else
			{
				isCulling = !isCulling;
				glEnable(GL_CULL_FACE);
				glutPostRedisplay();
				break;
			}
		case 'n':
			std::cout << std::boolalpha << "Normal normalization: " << !isNormalizingNormals << "\n";
			if(isNormalizingNormals)
			{
				isNormalizingNormals = !isNormalizingNormals;
				glDisable(GL_NORMALIZE);
				glutPostRedisplay();
				break;
			}
			else
			{
				isNormalizingNormals = !isNormalizingNormals;
				glEnable(GL_NORMALIZE);
				glutPostRedisplay();
				break;
			}
		case 'r':
			cleanup();
			initODE();
			glutPostRedisplay();
			break;
		case 't':
			/*for(int i = 0; terrain)
			{
			
			}*/
			isTexture = !isTexture;
	}
}

void loadHeightMap(const char *fn, std::vector<int> &vertices, int &h, int &w, int &m)
{
	cimg_library::CImg<int> image(fn);
	h = image.height;
	w = image.width;
	m = 0;
	for(int i = 0; i < h; ++i)
	{
		for(int j = 0; j < w; ++j)
		{
			vertices.push_back(image(j, i));
			if(image(j, i) > m)
				m = image(j, i);
		}
	}
}

void initGL()
{
	glEnable(GL_DEPTH_TEST);
	glEnable(GL_LIGHTING);
	glShadeModel(GL_SMOOTH);
	//glShadeModel(GL_FLAT);

	glEnable(GL_LIGHT0);
	GLfloat ambient0[] = {0.0f, 0.0f, 0.0f, 1.0f};
	glLightfv(GL_LIGHT0, GL_AMBIENT, ambient0);

	GLfloat diffuse0[] = {1.f, 1.f, 1.f, 1.0f};
	glLightfv(GL_LIGHT0, GL_DIFFUSE, diffuse0);

	GLfloat specular0[] = {0.9f, 0.9f, 0.9f, 1.0f};
	glLightfv(GL_LIGHT0, GL_SPECULAR, specular0);

	GLfloat position0[] = { 0.0f, 8.0f, 0.0f, 1.0f };
	glLightfv(GL_LIGHT0, GL_POSITION, position0);

	//glEnable(GL_LIGHT1);
	//GLfloat ambient1[] = {0.0f, 0.0f, 0.0f, 1.0f};
	//glLightfv(GL_LIGHT1, GL_AMBIENT, ambient1);

	//GLfloat diffuse1[] = {0.5f, 0.0f, 0.0f, 1.0f};
	//glLightfv(GL_LIGHT1, GL_DIFFUSE, diffuse1);

	//GLfloat specular1[] = {0.9f, 0.9f, 0.9f, 1.0f};
	//glLightfv(GL_LIGHT1, GL_SPECULAR, specular1);

	//GLfloat position1[] = { 0.0f, 5.0f, 4.0f, 1.0f };
	//glLightfv(GL_LIGHT1, GL_POSITION, position1);

	//glMaterialf(GL_FRONT, GL_SHININESS, 0.0f);

	glEnable(GL_COLOR_MATERIAL);
	glColorMaterial(GL_FRONT, GL_AMBIENT_AND_DIFFUSE);
}


Box* getParticle(int x, int y) {return particles[y*num_particles_width + x];}
std::vector <ParticleConstraint> particleConstraints;
void initODE()
{
	////////////////////////////////////////////////////////////	
	// Create world, collision space and contact group	
	////////////////////////////////////////////////////////////
	dInitODE(); //If this is uncommented so must be dCloseODE()

	world = dWorldCreate();

	space = dHashSpaceCreate(0);

	// Set up gravity force
	dWorldSetGravity (world,0,-9.81,0); // Y axis is up/down

	// Automatically disable stationary object checking
	dWorldSetAutoDisableFlag(world, 1);
	dWorldSetAutoDisableLinearThreshold(world, 0.1);
	dWorldSetAutoDisableAngularThreshold(world, 0.1);
	
	// Set error correction (bigger numbers means more)
	dWorldSetERP(world, 0.8);

	// Damp the world
	dWorldSetDamping(world, 0.001, 0.001);

	// Create contact group	
	contactGroup = dJointGroupCreate(0); 
	
	////////////////////////////////////////////////////////////	
	// Creating objects
	////////////////////////////////////////////////////////////s	

	Box *box = new Box(world, space, 0.5, 0.5, 0.5, 2.0);	
	box->setPosition(-.7, 2, 0);	
	box->color[0] = 255;
	box->color[1] = 0;
	box->color[2] = 0;
	objects.push_back(box);

	if(usePlanes)
	{
		// Create static planes	
		planes.push_back(dCreatePlane(space, 0, 1, 0, -1.0));
	}
	else
	{ //Use trimesh from file
		//ImgReader imgR;
/*
		vector <int>vertices; //all the vertices values to be stored.
		int h = 0, w = 0, m = 0; //height, width, m is the maximum gray value . between[0,65536]
		loadHeightMap("sink2.pgm", vertices, h, w, m);
		
		minTerrainHeight = m*0.5f*-1.0f;
		//Height of the image become depth of the mesh
		Terrain *terrain = new Terrain(space, vertices, h, w, 1.0f, 0.5f, 1.0f, 
															-0.0f, -m*0.5f, -0.0f, true);
		terrain->color[0] = 25;
		terrain->color[1] = 25;
		terrain->color[2] = 25;
		terrain->useTexture = false;
		staticTrimeshes.push_back(terrain);
	*/
	}



/*
		particles.resize(num_particles_width*num_particles_height); //I am essentially using this vector as an array with room for num_particles_width*num_particles_height particles

		// creating particles in a grid of particles from (0,0,0) to (width,-height,0)
		for(int x=0; x<num_particles_width; x++)
		{
			for(int y=0; y<num_particles_height; y++)
			{
				//each particile can be a sphere 
				Vec3 pos = Vec3(width * (x/(float)num_particles_width),
								-height * (y/(float)num_particles_height),
								0);
				particles[y*num_particles_width+x]= Particle(pos); // insert particle in column x at y'th row
			}
		}
*/
	
	particles.resize(num_particles_width*num_particles_height);
	for(int x=0; x<num_particles_width; x++)
	{
		for(int y=0; y<num_particles_height; y++)
		{
		//each particile can be a sphere 
			//glVertex3fv(((cml::vector3f)(planeOrigin - size * spanVect1 - size * spanVect2)).data());
			//glVertex3fv pos = glVertex3fv(
			float pos[3];
			pos[0] = width * (x/(float)num_particles_width);
			pos[1] = -height * (y/(float)num_particles_height);
			pos[2] =0;
			Box *box = new Box(world, space, boxw, boxh, boxd, 0.0001);
			if(x==0 && y ==0) dBodyDisable(box->body);
			box->setPosition(pos[0],pos[1],pos[2]);
			box->color[0]= 100;
			box->color[1]= 100;
			box->color[2]= 100;
			particles[y*num_particles_width+x] = box;
			objects.push_back(box);						
		}
	}
	
	//Particle* getParticle(int x, int y) {return &particles[y*num_particles_width + x];}
/*
		for(int x=0; x<num_particles_width; x++)
		{
			for(int y=0; y<num_particles_height; y++)
			{
				if (x<num_particles_width-1) makeConstraint(getParticle(x,y),getParticle(x+1,y));
				if (y<num_particles_height-1) makeConstraint(getParticle(x,y),getParticle(x,y+1));
				if (x<num_particles_width-1 && y<num_particles_height-1) makeConstraint(getParticle(x,y),getParticle(x+1,y+1));
				if (x<num_particles_width-1 && y<num_particles_height-1) makeConstraint(getParticle(x+1,y),getParticle(x,y+1));
			}
		}
*/
					
		for(int x=0; x<num_particles_width; x++)
		{
			for(int y=0; y<num_particles_height; y++)
			{
				if (x<num_particles_width-1){
	
				const dReal *r1 =	dBodyGetPosition( getParticle(x,y)->body); // - dBodyGetPosition( getParticle(x+1,y)->body);
				const dReal *r2 =	dBodyGetPosition( getParticle(x+1,y)->body);
				Vec3 *v1 = (Vec3 *)r1;
				Vec3 *v2 = (Vec3 *)r2;	
					if (x<num_particles_width-1){
						ParticleConstraint pc(getParticle(x,y),getParticle(x+1,y));
						particleConstraints.push_back(pc);
					}
					if (y<num_particles_height-1){
						ParticleConstraint pc(getParticle(x,y),getParticle(x,y+1));
						particleConstraints.push_back(pc);
					}
					if (x<num_particles_width-1 && y<num_particles_height-1){
						ParticleConstraint pc(getParticle(x,y),getParticle(x+1,y+1));
						particleConstraints.push_back(pc);
					}
					
					if (x<num_particles_width-1 && y<num_particles_height-1){
						ParticleConstraint pc(getParticle(x+1,y),getParticle(x,y+1));
						particleConstraints.push_back(pc);

					}


				}
			}
		}

		for(int x=0; x<num_particles_width; x++)
		{
			for(int y=0; y<num_particles_height; y++)
			{
				if (x<num_particles_width-2){
					ParticleConstraint pc(getParticle(x,y),getParticle(x+2,y));
					particleConstraints.push_back(pc);
				}
				if (y<num_particles_height-2){
					ParticleConstraint pc(getParticle(x,y),getParticle(x,y+2));
					particleConstraints.push_back(pc);
				}
				if (x<num_particles_width-2 && y<num_particles_height-2){
					ParticleConstraint pc(getParticle(x,y),getParticle(x+2,y+2));
					particleConstraints.push_back(pc);
				}
				if (x<num_particles_width-2 && y<num_particles_height-2){
					ParticleConstraint pc(getParticle(x+2,y),getParticle(x,y+2));
					particleConstraints.push_back(pc);
				}
			}
		}

/*
		// Connecting secondary neighbors with constraints (distance 2 and sqrt(4) in the grid)
		for(int x=0; x<num_particles_width; x++)
		{
			for(int y=0; y<num_particles_height; y++)
			{
				if (x<num_particles_width-2) makeConstraint(getParticle(x,y),getParticle(x+2,y));
				if (y<num_particles_height-2) makeConstraint(getParticle(x,y),getParticle(x,y+2));
				if (x<num_particles_width-2 && y<num_particles_height-2) makeConstraint(getParticle(x,y),getParticle(x+2,y+2));
				if (x<num_particles_width-2 && y<num_particles_height-2) makeConstraint(getParticle(x+2,y),getParticle(x,y+2));			}
		}

*/


	//now create constraits and satisfy the constraints.

	
}

void renderPlane(dVector4 equation, float size)
{
	cml::vector3f planeNorm = cml::vector3f(equation[0], equation[1], equation[2]);
	planeNorm.normalize();
	cml::vector3f planeOrigin = equation[3] * planeNorm;

	//These calculations are done in the process of figuring out a parallel vector to the normal
	float v0 = max(abs(planeNorm[0] - 1.0), abs(planeNorm[0] + 1.0)); //1.0 is used here as the length of the planeNorm, we could also use planeNorm.length()
	cml::vector3f v = cml::vector3f(v0, planeNorm[1], planeNorm[2]);
	float V = v[0] * v[0] + v[1] * v[1] + v[2] + v[2];

	//Find first orthogonal vector to normal (believe me on the formula)
	cml::vector3f spanVect1 = cml::vector3f(-2.0 * v[1] * v[0] / V, 1.0 - 2.0 * v[1] * v[1] / V, -2.0 * v[2] * v[1] / V);
	//Find second using cross product of first two
	cml::vector3f spanVect2 = cross(planeNorm, spanVect1).normalize();

	glColor3f(1.0, 1.0, 1.0);
	GLfloat mat_diffuse[] = {1.0f, 1.0f, 1.0f};
	glMaterialfv(GL_FRONT, GL_DIFFUSE, mat_diffuse);

	GLfloat mat_specular[] = {0.0f, 0.0f, 0.0f};
	glMaterialfv(GL_FRONT, GL_SPECULAR, mat_specular);
	glBegin(GL_QUADS);
		glNormal3fv(planeNorm.data());
		glVertex3fv(((cml::vector3f)(planeOrigin - size * spanVect1 - size * spanVect2)).data());
		glVertex3fv(((cml::vector3f)(planeOrigin + size * spanVect1 - size * spanVect2)).data());
		glVertex3fv(((cml::vector3f)(planeOrigin + size * spanVect1 + size * spanVect2)).data());
		glVertex3fv(((cml::vector3f)(planeOrigin - size * spanVect1 + size * spanVect2)).data());
	glEnd();
	//printf("%f, %f, %f, %f\n", equation[0], equation[1], equation[2], equation[3]);
}

void handleCollisionBetween(dGeomID o0, dGeomID o1)
{
	// Create an array of dContact objects to hold the contact joints
	static const int MAX_CONTACTS = 10;
	dContact contact[MAX_CONTACTS];

	for (int i = 0; i < MAX_CONTACTS; i++)
	{
		contact[i].surface.mode = dContactBounce | dContactSoftCFM | dContactApprox1;
		contact[i].surface.mu = 0.9;
		contact[i].surface.mu2 = 0.9;
		contact[i].surface.bounce = 0.8;
		contact[i].surface.bounce_vel = 0.1;
		contact[i].surface.soft_cfm = 0.01;
	}
	if (int numc = dCollide(o0, o1, MAX_CONTACTS, &contact[0].geom, sizeof(dContact)))
	{
		// Get the dynamics body for each geom
		dBodyID b1 = dGeomGetBody(o0);
		dBodyID b2 = dGeomGetBody(o1);
		// To add each contact point found to our joint group we call dJointCreateContact which is just one of the many
		// different joint types available.  
		for (int i = 0; i < numc; i++)
		{
			// dJointCreateContact needs to know which world and joint group to work with as well as the dContact
			// object itself. It returns a new dJointID which we then use with dJointAttach to finally create the
			// temporary contact joint between the two geom bodies.
			dJointID c = dJointCreateContact(world, contactGroup, contact + i);
			dJointAttach(c, b1, b2);
		}
	}	
}

//Callback method for collision
void nearCallback(void *data, dGeomID o0, dGeomID o1)
{
	handleCollisionBetween(o0,o1);
}
//ball animation
float ball_time = 0;
Cloth cloth1(14,10,55,45); // one Cloth object of the Cloth class
Vec3 ball_pos(7,-5,0); // the center of our one ball
float ball_radius = 2; // the radius of our one ball

void renderScene()
{

	//Cloth set up.. from DisplayMethod()
	ball_time++;
	ball_pos.f[2] = cos(ball_time/50.0)*7;

	cloth1.addForce(Vec3(0,-0.2,0)*TIME_STEPSIZE2); // add gravity each frame, pointing down
	cloth1.windForce(Vec3(0.5,0,0.2)*TIME_STEPSIZE2); // generate some wind each frame
	cloth1.timeStep(); // calculate the particle positions of the next frame
	cloth1.ballCollision(ball_pos,ball_radius); // resolve collision with the ball

	//here apply the satisfy constraint.

	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	glMatrixMode( GL_MODELVIEW );	// Set the view matrix ...
	glLoadIdentity();				// ... to identity.
    gluLookAt(0.0,0.0,5.0, 
		      0.0,0.0,-1.0,
			  0.0f,1.0f,0.0f); // camera is on the z-axis
	glTranslatef(0, 0, -g_trackball_zoom);
	glTranslatef(g_trackball_translation_x, g_trackball_translation_y, 0);
	glRotatef(g_trackball_model_rot_x, 1, 0, 0);
	glRotatef(g_trackball_model_rot_y, 0, 1, 0);

	unsigned int i;

	for(i = 0; i < planes.size(); ++i)
	{
		if (dGeomGetClass(planes[i]))
		{
			dVector4 equation;
			dGeomPlaneGetParams(planes[i], equation);
			renderPlane(equation, 5);
		}
	}

	
	for(i = 0; i < objects.size(); ++i)
	{
		objects.at(i)->render();
	}


	for(i = 0; i < staticTrimeshes.size(); ++i)
	{
		staticTrimeshes.at(i)->render();
		//printf("Drawing");
	}


	//animate cloth and ball
	glTranslatef(-6.5,6,-9.0f); // move camera out and center on the cloth
	glRotatef(25,0,1,0); // rotate a bit to see the cloth from the side
	cloth1.drawShaded(); // finally draw the cloth with smooth shading
	


	//satisfy constraint
	std::vector<ParticleConstraint>::iterator it ;

	for(it = particleConstraints.begin(); it < particleConstraints.end();it++)
	{
		//check its height, if it is higher than terrain then 
		//if((*it).p1->getPosition()[1]> (minTerrainHeight) && (*it).p2->getPosition()[1]> (minTerrainHeight))
			//if( dBodyIsEnabled((*it).p1->body)==1)	
				
				(*it).satisfyConstraint();
	
	}


	glPushMatrix(); // to draw the ball we use glutSolidSphere, and need to draw the sphere at the position of the ball
	glTranslatef(ball_pos.f[0],ball_pos.f[1],ball_pos.f[2]); // hence the translation of the sphere onto the ball position
	glColor3f(0.4f,0.8f,0.5f);
	glutSolidSphere(ball_radius-0.1,50,50); // draw the ball, but with a slightly lower radius, otherwise we could get ugly visual artifacts of cloth penetrating the ball slightly
	glPopMatrix();

	glutSwapBuffers();

	glFlush();
}


void animate(int value)
{
	//Setup another call to animate in <animationUpdateRate> miliseconds
	glutTimerFunc(animationUpdateRate, animate, 0);

	//Ask for display method to be called
	glutPostRedisplay();
}

void simulatePhysics(int value)
{
	//Setup another call to simulatePhysics in <physicsUpdateRate> miliseconds
	glutTimerFunc(physicsUpdateRate, simulatePhysics, 0);

	// Detect collision
	dSpaceCollide(space, NULL, &nearCallback);

	//This line caused issues on non-Bryce computers, if you don't see anything
	//try changing the number to something like 0.005
	dWorldQuickStep(world, 1.0 * (physicsUpdateRate / 1000.0) );

	// Remove all temporary collision joints now that the world has been stepped
	dJointGroupEmpty(contactGroup);	
}

//If window size is changed
void sizeChanged(int w, int h)
{
	if(h == 0)
		h = 1;

	double ratio = 1.0 * w / h;

	//Reset the coordinate system before modifying
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();

	//Set the viewport to be the entire window
	glViewport(0, 0, w, h);

	//Set the correct perspective.
	gluPerspective(45, ratio, 0.001f, 1000.0f);
}	

//Cleanup method for when program exits
void cleanup()
{
	Object *tempObject;
	while(!objects.empty())
	{
		tempObject = objects.back();
		objects.pop_back();
		delete tempObject;
	}
	
	StaticTrimesh *tempTrimesh;
	while(!staticTrimeshes.empty())
	{
		tempTrimesh = staticTrimeshes.back();
		staticTrimeshes.pop_back();
		delete tempTrimesh;
	}

	dJointGroupDestroy(contactGroup);
	dSpaceDestroy(space);
	dWorldDestroy(world);
	dCloseODE(); //If this is uncommented so must be dInitODE()
}

int main(int argc, char* argv[])
{
	atexit(cleanup); //Run cleanup routine when program terminates
	glutInit(&argc, argv);
	glutInitWindowPosition(0, 0);
	glutInitWindowSize(600, 400);
	glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE | GLUT_DEPTH);
	glutCreateWindow("Code1 Window");

	glutDisplayFunc(renderScene);
	glutTimerFunc(animationUpdateRate, animate, 0);
	glutTimerFunc(physicsUpdateRate, simulatePhysics, 0);
	glutReshapeFunc(sizeChanged);

	initGL();

	startTime = glutGet(GLUT_ELAPSED_TIME);
	prevTime = startTime;

	glutMouseFunc(handleMouseClick);		// Set function to handle mouse clicks
	glutMotionFunc(handleMouseMotion);		// Set function to handle mouse motion
	glutKeyboardFunc(handleKeyboardEvent);

	initODE();

	glutMainLoop();

	return 0;
}
